% CORDIC - COordinate Rotation DIgital Computer
% This method can be used to calculate trigonometric functions
% more efficient way than the standard routines. Especially
% when high precisions not required. Following examples shows
% the CORDIC to calculate cos(x), sin(x) and atan(y).
% 
% CORDIC is based on rotation matrix applied on vector. See wiki
% for detailed information and the attached image (cordic.gif). The
% key assumption is that angle Theta can be be split into more parts
% that are simply added. E.g. T=a1+a2. Translated into rotation:
% 
%   |cos(T) -sin(T)|     |cos(a1) -sin(a1)|   |cos(a2) -sin(a2)|
%   |sin(T)  cos(T)|  =  |sin(a1)  cos(a1)| * |sin(a2)  cos(a2)|
% 
% Also we need to obtain a predefined cos(a) and sin(a) for a very
% small number and that just apply the rotation number of times.
% 
% Calculate atan(y)
% 
%   [1,y] * |cos(a) -sin(a)|
%           |sin(a)  cos(a)|
% 
% where y is the tan(a). Keep rotating the vector until you get
% y = 0. Then remember the angle you used to rotate the vector.
% 
% Compute sin(x) cos(x). Use the vector [1,0] and rotate it repeatedly.
% The number of cycles is given by the ration angle/step, where angle
% is the angle we want to calculate and the step is chosen angle, for
% which we know the sin(step) and cos(step). The further simplification
% can be achieved by extracting cos() from the rotation matrix:
% 
%  cos(x) * |   1    -tan(x)|
%           | tan(x)    1   |
% 
% That simplifies the matrix remarkably and speeds up the operation.
% 
% 
% Define the base rotation table for angle pi/60 ~ 3 deg.
M=[cos(pi/60),-sin(pi/60);sin(pi/60),cos(pi/60)];
V=[0,1];

% Find sin(30):
t=V;
for i = 1 : 1 : 10 % (rotate 10x)
	t=t*M;
end
disp("CORDIC Sine and Cosine:");
disp(t); % display the result
disp("Implicit Sine and Cosine:");
t(1,1)=sin(30/180*pi); % check the displayed result for implicit implementation
t(1,2)=cos(30/180*pi);
disp(t);

%Unit tests for inverse CORDIC functions
disp("Unit Tests for CORDIC functions");
disp("atan(tan(49.43))");
cordic_atan(tan(49.43*pi/180))*180/pi
disp("asin(sin(49.43))");
cordic_asin(sin(49.43*pi/180))*180/pi
disp("acos(cos(49.43))");
cordic_acos(cos(49.43*pi/180))*180/pi

disp("atan(tan(7.28))");
cordic_atan(tan(7.28*pi/180))*180/pi
disp("asin(sin(7.28))");
cordic_asin(sin(7.28*pi/180))*180/pi
disp("acos(cos(7.28))");
cordic_acos(cos(7.28*pi/180))*180/pi

disp("atan(tan(71.89))");
cordic_atan(tan(71.89*pi/180))*180/pi
disp("asin(sin(71.89))");
cordic_asin(sin(71.89*pi/180))*180/pi
disp("acos(cos(71.89))");
cordic_acos(cos(71.89*pi/180))*180/pi
